#include "KDTree.h"
#include <ctime>
#include <vector>
#include <cstdlib>
#include <algorithm>
#include <math.h>
#include <set>

KDTree::KDTree(vector<Photon*> *&photons, int depth)
{
    _splitplane = depth % 3;
    _internal = photons;
    _depth = depth;
    CreateNewTree();
    max_distance = 100.0;
}

bool KDTree::CreateNewTree()
{
    float split_value = FindMedian();
    //cout << split_value << endl;
    _photon = NULL;
    vector<Photon*> *left, *right;
    left = new vector<Photon*>;
    right = new vector<Photon*>;
    for (int i = 0; i < _internal->size(); i++)
    {
        //cout << photons[i]._position[_splitplane] << " " << split_value << endl;
        if ((*_internal)[i]->_position[_splitplane] == split_value)
        {
            if (_photon == NULL)
            {
                _photon = (*_internal)[i];
            }
            else
            {
                if (left->size() < right->size())
                    left->push_back((*_internal)[i]);
                else
                    right->push_back((*_internal)[i]);
            }
        }
        else if ((*_internal)[i]->_position[_splitplane] > split_value)
            left->push_back((*_internal)[i]);
        else
            right->push_back((*_internal)[i]);
    }

    //cout << "Created at depth: " << _depth << " median: " << split_value << " left: " << left->size() << " right: " << right->size() << " total: " << left->size() + right->size() << " original: " <<  _internal->size() << " with split plane: " << _splitplane << endl;

    _depth++;
    _left = _right = NULL;

    if (left->size() > 0)
        _left = new KDTree(left, _depth);

    delete left;

    if (right->size() > 0)
        _right = new KDTree(right, _depth);

    delete right;
}

float KDTree::FindMedian()
{
    int list_length = _internal->size();
    float random = 0;
    int minimum = 1000;
    // Minimum or 10% of all samples.
    int samples = (int)(max(minimum, (int) ((float)list_length * 0.00001)));
    samples = min(samples, list_length);
    vector<float> * median_list = new vector<float>;

    int sample_value;

    for (int i = 0; i < samples; i++)
    {
        random = rand();
        random /= RAND_MAX;
        sample_value = list_length * random;
        //cout << "MAX: " << list_length << " value: " << sample_value << endl;
        if (sample_value < _internal->size() && sample_value >= 0)
        {
            median_list->push_back(_internal->at(sample_value)->_position[_splitplane]);
        }
    }
    sort(median_list->begin(), median_list->end());


    //cout << "median: " << samples / 2 << " : " << median_list[samples / 2] << endl;
    float med =  (*median_list)[median_list->size() / 2];

    delete median_list;

    return med;
}

bool KDTest (const KDNode& k1, const KDNode& k2)
{
    return k1.dist > k2.dist;
}


void KDTree::FindKdTrees(Vector3 point, KDTree &search, multiset<KDNode, ClassKDTest> &result, int N)
{
    if (&search == NULL)
        return;

    int current_split;

    current_split = search._splitplane;
    KDNode kdn;
    kdn._ph = search._photon;
    kdn.dist = (kdn._ph->_position - point).LengthPow2();
    float this_dist = pow(search._photon->_position[current_split] - point[current_split], 2.0);
    float max_dist = max_distance;

    if (result.size() <= N || kdn.dist < (*(--result.end())).dist)
    {
            result.insert(kdn);
            //cout << result.size() << " inserted: " << kdn.dist << " max: " << (*(--result.end())).dist << endl;
    }


    if (result.size() > N)
    {
        result.erase(--result.end(), result.end());
        //cout << "erase" << endl;
    }



    if (result.size() >= N)
    {
        //cout << N/5 << endl;
        max_dist = min(max_dist, (*(--result.end())).dist);
        //cout << "max: " << max_dist << endl;
    }

    if (search._photon->_position[current_split] < point[current_split])
    {
        FindKdTrees(point, *search._left, result, N);
        if ( this_dist < max_dist)
        {
            //cout << "invest right" << endl;
            FindKdTrees(point, *search._right, result, N);

            //investigate.push_back(search->_right);
        } //else  cout << "NO INVEST: " << max_dist << endl;
    }
    else
    {
        if (search._right != NULL)
        FindKdTrees(point, *search._right, result, N);

        if ( this_dist < max_dist)
        {
            //cout << "invest left" << endl;

            FindKdTrees(point, *search._left, result, N);

            //investigate.push_back(current->_left);
            //investigate.push_back(current->_right);
            //cout << "invest" << endl;
        }
        //else { cout << "NO INVEST: " << max_dist << endl;}
    }
}


vector<Photon*> KDTree::FindClosestPhotons(Vector3 point, int N, float newMax)
{

    max_distance = newMax;

    vector<KDNode> all;
    multiset<KDNode, ClassKDTest> nodes;
    FindKdTrees(point, *this, nodes, N);

    //cout << "ut: " << nodes.size() << endl << endl << endl;

    vector<Photon*> temp;


    set<KDNode, ClassKDTest>::iterator itr;
    //itr = itr++;

        //sort(all.begin(), all.end(), KDTest);
    //nodes.erase(nodes.begin() + min(N, (int) nodes.size()), nodes.end());

    int i = 0;

    for (multiset<KDNode, ClassKDTest>::iterator itr = nodes.begin(); itr != nodes.end(); itr++)
    {
        if (i > N)
            break;
      //  cout << (*itr).dist << endl;
        temp.push_back((*itr)._ph);
        i++;
    }

    //cout << max_distance << endl;

    //cout << endl << endl;

    //Skicka point till FindKdTrees()

    //Gå genom svaret o plocka Photoner från KDTrees.


//    vector<KDNode> result;
//    vector<KDTree*> investigate;
//
//    KDTree * current;
//    current = this;
//    int current_split;
//
//    while (current != NULL)
//    {
//        current_split = current->_splitplane;
//        KDNode kdn;
//        kdn._ph = current->_photon;
//        kdn.dist = (current->_photon->_position - point).LengthPow2();
//        result.push_back(kdn);
//        //cout << "split: " << current_split << " position:" << current->_photon->_position << " dist: " << (current->_photon->_position - point).length() << endl;
//        if (current->_photon->_position[current_split] < point[current_split])
//        {
//            if (current->_left != NULL && pow(current->_left->_photon->_position[current->_left->_splitplane] - point[_splitplane], 2.0) < 1.0)
//            {
//                investigate.push_back(current->_right);
//                //cout << "invest" << endl;
//            }
//            current = current->_left;
//        }
//        else
//        {
//            if (current->_right != NULL && pow(current->_right->_photon->_position[current->_right->_splitplane] - point[_splitplane], 2.0) < 1.0)
//            {
//                investigate.push_back(current->_left);
//                //cout << "invest" << endl;
//            }
//            current = current->_right;
//        }
//
//    }
//
//    //cout << "done" << endl;
//
//    vector<Photon*> temp;
//    result.erase(result.begin() + min(N, (int) result.size()), result.end());
//
//    for (int i = 0; i < result.size(); i++)
//    {
////        cout << result[i].dist << endl;
//        temp.push_back(result[i]._ph);
//    }
//    //cout << endl << endl;

    return temp;
}

    /*KDTree * current;
    current = this;
    vector<KDTree*> investigate;
    int current_split;
    float max_dist = 18.4;

    while (current != NULL)
    {
        current_split = current->_splitplane;
        KDNode kdn;
        kdn._ph = current->_photon;
        kdn.dist = (current->_photon->_position - point).LengthPow2();


        //if (result.size() <= N || kdn.dist < (*(--result.end())).dist && kdn.dist < 2*max_dist)
        //{
            result.insert(kdn);
            //cout << result.size() << " inserted: " << kdn.dist << " max: " << (*(--result.end())).dist << endl;
        //}
/*
        if (result.size() > N)
        {
            result.erase(--result.end(), result.end());
            max_dist = min(max_dist, (*(--result.end())).dist);
        }*/
/*
        if (current->_photon->_position[current_split] < point[current_split])
        {
            if ( pow(current->_photon->_position[current_split] - point[current_split], 2.0) < max_dist)
            {
                investigate.push_back(current->_right);
                //investigate.push_back(current->_right);
                //cout << "invest" << endl;
            }
            current = current->_left;
        }
        else
        {
            if ( pow(current->_photon->_position[current_split] - point[current_split], 2.0) < max_dist)
            {
                investigate.push_back(current->_left);
                //investigate.push_back(current->_right);
                //cout << "invest" << endl;
            }
            current = current->_right;
        }
    }

    for (int i = 0; i < investigate.size(); i++)
    {
        investigate[i]->FindKdTrees(point, result, N);
    }*/
